#include <math.h>
#include <cstdint>
#include <cstdio>

//终端下编译指令：
//g++ -o metis_vp.so -shared -fPIC metis_vp.cpp
//-o 指定生成的文件名，-shared 指定微共享库，-fPIC 表明使用地址无关代码

/*在extern "C"中的函数才能被外部调用 */
extern "C"
{
    /* PID控制句柄结构体 */
    typedef struct
    {
        float_t kp;         // 比例控制参数
        float_t ki;         // 积分控制参数
        float_t kd;         // 微分控制参数
        float_t error;      // 当前误差
        float_t error_1;    // 历史误差1
        float_t error_2;    // 历史误差2
        float_t quantity;   // 调整量
    } PIDHandler;

/*_______________param_adjustment____________*/
/* 1. 图像控制参数 */
#define SCREEN_WIDTH                480     //图像宽度
#define SCREEN_HEIGHT               180     //图像高度
#define BIN_THRESHOLD               20      //二值化阈值
#define CUTDOWN_THRESHOLD           90      //由黑向白遍历截止阈值

/* 2. 前向速度参数 */
#define FORWART_SPEED_NORMAL        100      //正常前进速度
#define FORWART_SPEED_TURNING       80      //弯道减速速度
#define FORWART_SPEED_SLOWDOWN      40      //击靶前减速速度

/* 3. 转速参数 */
#define TURN_SPEED_MODE1_FLEXIBLE   200     //灵活模式下转向限制
#define TURN_SPEED_MODE2_STABLE     180     //标准模式下转向限制
#define TURN_SPEED_MODE3_SLOW       80      //低速模式下转向限制
#define TURN_SPEED_MAX_STEP         60      //转向最大改变幅度
#define BENDING_DECELERATION        40      //弯道减速阈值（转向速度过高则设置弯道减速）

/* 4. 时域控制参数 */
#define TIMENODE_MODE1              2000
#define TIMENODE_MODE2              6000
#define TIMENODE_SLOWDOWN           7500

/* 5. PID控制器参数 */
    PIDHandler metis_PID = {
        .kp = 2,
        .ki = 0,
        .kd = 2};

    /**
     * @brief PID更新函数，PID算法公式的具体实现
     * @return NONE
     */
    void PID_Update(PIDHandler *PID)
    {
        PID->quantity = 0;
        PID->quantity += PID->kp * PID->error;
        PID->quantity += PID->ki * (PID->error + PID->error_1 + PID->error_2);
        PID->quantity += PID->kd * (PID->error - PID->error_1);
        PID->error_2 = PID->error_1;
        PID->error_1 = PID->error;
    }

    /**
     * @brief 检测黑白格子
     * 
     * @param frameData 
     * @param sign_up 
     * @param sign_down 
     * @return int 
     */
    int detectCheck(int frameData[SCREEN_HEIGHT][SCREEN_WIDTH])
    {
        int cnt_col[475] = {0};
        int sign_up = 0;
        int sign_down = 0;
        int flag_row = 0, flag_col = 0;
        for (int i = 90; i < SCREEN_HEIGHT; i++)
        {
            flag_col = 0;
            for (int j = 0; j < 475; j++)
            {
                if (frameData[i][j] - frameData[i][j + 5] > 30)
                    flag_col++;
                //if(frameData[i][j+5] - frameData[i][j+5] > 50)
                //flag_col ++;
            }
            cnt_col[flag_col]++;
            if (flag_col > 40 && flag_col <80)
                flag_row++;
            if (i >= 145 && flag_col > 10)
                sign_down = 1;
            if (i == 20 && flag_col == 0)
                sign_up = 1;
        }
        int max_col=0;
        for(int i=0;i<475;i++)
        {
            if(cnt_col[max_col]<cnt_col[i])
                max_col =i;
        }
        if (1)
            return max_col;
        else
            return -1;
    }

    /**
     * @brief Get the error object
     * 
     * @param frameData 
     * @return int 
     */
    int get_error(int frameData[SCREEN_HEIGHT][SCREEN_WIDTH])
    {
        int b_left = 0, b_right = 0;
        /* left pixels */
        for (size_t i = 0; i < SCREEN_HEIGHT; i++)
        {
            for (size_t j = 0; j < SCREEN_WIDTH / 2 - 10; j++)
            {
                if (frameData[i][j] < BIN_THRESHOLD)
                    b_left++;
                if (frameData[i][j] > CUTDOWN_THRESHOLD)
                    break;
                
            }
        }
        /* right pixels */
        for (size_t i = 0; i < SCREEN_HEIGHT; i++)
        {
            for (size_t j = SCREEN_WIDTH - 1; j >= SCREEN_WIDTH / 2 + 10; j--) //250
            {
                if (frameData[i][j] < BIN_THRESHOLD)
                    b_right++;
                if (frameData[i][j] > CUTDOWN_THRESHOLD)
                    break;
            }
        }
        /* calculate error */
        if (b_right - b_left >= 0)
            return sqrt(b_right - b_left);
        else
            return -sqrt(b_left - b_right);
    }

    /**
     * @brief 
     * 
     * @param frameData 
     * @param v_turn 
     * @param v_run 
     * @param err 
     * @param flag 
     * @param slowdown 
     */
    void control(   int frameData[SCREEN_HEIGHT][SCREEN_WIDTH],
                    int *v_turn,
                    int *v_run,
                    int *eventFlag,
                    int timeStamp
                    )
    {
        
        int flag_row, whiterow;
        static int v_turn_last = 0;
        /* 1. 计算方向误差值 */
        metis_PID.error = (float)get_error(frameData);
    
        /* 2. PID算法更新 */
        PID_Update(&metis_PID);
        *v_turn = metis_PID.quantity;
        v_turn_last = *v_turn;

        /* 3. 在不同状态下限制v_turn 的幅值 */
        if (abs(*v_turn) > TURN_SPEED_MODE3_SLOW && timeStamp>TIMENODE_MODE2)
            *v_turn = *v_turn > 0 ? TURN_SPEED_MODE3_SLOW : -TURN_SPEED_MODE3_SLOW;

       /* if (abs(*v_turn) > TURN_SPEED_MODE1_FLEXIBLE && timeStamp<TIMENODE_MODE1)
            *v_turn = *v_turn > 0 ? TURN_SPEED_MODE1_FLEXIBLE : -TURN_SPEED_MODE1_FLEXIBLE;

        else if (abs(*v_turn) > TURN_SPEED_MODE2_STABLE && timeStamp <TIMENODE_MODE2)
            *v_turn = *v_turn > 0 ? TURN_SPEED_MODE2_STABLE : -TURN_SPEED_MODE2_STABLE;

        else if (abs(*v_turn) > TURN_SPEED_MODE3_SLOW)
            *v_turn = *v_turn > 0 ? TURN_SPEED_MODE3_SLOW : -TURN_SPEED_MODE3_SLOW;*/

        /* 4. 设置弯道减速 */
        *v_run = FORWART_SPEED_NORMAL;
        if (abs(*v_turn) > BENDING_DECELERATION) //可调 直道
            *v_run = FORWART_SPEED_TURNING;
        if(timeStamp>TIMENODE_SLOWDOWN)
            *v_run = FORWART_SPEED_SLOWDOWN;
        /* 5. 判断直道加速和黑白棋盘停止击靶 */
        flag_row = detectCheck(frameData);
        *eventFlag = flag_row;
    }
}
